#include <algorithm>
#include "camera.h"

const float Camera::DEFAULT_ROTATION_SPEED = 0.3f;
const float Camera::DEFAULT_FOVX = 90.0f;
const float Camera::DEFAULT_ZNEAR = 0.1f;
const float Camera::DEFAULT_ZFAR = 1000.0f;

const Vector3 Camera::WORLD_XAXIS(1.0f, 0.0f, 0.0f);
const Vector3 Camera::WORLD_YAXIS(0.0f, 1.0f, 0.0f);
const Vector3 Camera::WORLD_ZAXIS(0.0f, 0.0f, 1.0f);

Camera::Camera() {
	m_accumPitchDegrees = 0.0f;
	m_savedAccumPitchDegrees = 0.0f;

	m_rotationSpeed = DEFAULT_ROTATION_SPEED;
	m_fovx = DEFAULT_FOVX;
	m_aspectRatio = 0.0f;
	m_znear = DEFAULT_ZNEAR;
	m_zfar = DEFAULT_ZFAR;

	m_eye.set(0.0f, 0.0f, 0.0f);
	m_savedEye.set(0.0f, 0.0f, 0.0f);
	m_target.set(0.0f, 0.0f, 0.0f);
	m_xAxis.set(1.0f, 0.0f, 0.0f);
	m_yAxis.set(0.0f, 1.0f, 0.0f);
	m_targetYAxis.set(0.0f, 1.0f, 0.0f);
	m_zAxis.set(0.0f, 0.0f, 1.0f);
	m_viewDir.set(0.0f, 0.0f, -1.0f);

	m_acceleration.set(0.0f, 0.0f, 0.0f);
	m_currentVelocity.set(0.0f, 0.0f, 0.0f);
	m_velocity.set(0.0f, 0.0f, 0.0f);

	m_orientation.identity();
	m_savedOrientation.identity();

	m_viewMatrix.identity();
	m_projMatrix.identity();
	m_viewProjMatrix.identity();
}

Camera::~Camera() {
}

void Camera::lookAt(const Vector3 &target) {
	lookAt(m_eye, target, m_yAxis);
}

void Camera::lookAt(const Vector3 &eye, const Vector3 &target, const Vector3 &up) {
	m_eye = eye;
	m_target = target;

	m_zAxis = eye - target;
	m_zAxis.normalize();

	m_viewDir = -m_zAxis;

	m_xAxis = Vector3::cross(up, m_zAxis);
	m_xAxis.normalize();

	m_yAxis = Vector3::cross(m_zAxis, m_xAxis);
	m_yAxis.normalize();
	m_xAxis.normalize();

	m_viewMatrix[0][0] = m_xAxis.x;
	m_viewMatrix[1][0] = m_xAxis.y;
	m_viewMatrix[2][0] = m_xAxis.z;
	m_viewMatrix[3][0] = -Vector3::dot(m_xAxis, eye);

	m_viewMatrix[0][1] = m_yAxis.x;
	m_viewMatrix[1][1] = m_yAxis.y;
	m_viewMatrix[2][1] = m_yAxis.z;
	m_viewMatrix[3][1] = -Vector3::dot(m_yAxis, eye);

	m_viewMatrix[0][2] = m_zAxis.x;
	m_viewMatrix[1][2] = m_zAxis.y;
	m_viewMatrix[2][2] = m_zAxis.z;
	m_viewMatrix[3][2] = -Vector3::dot(m_zAxis, eye);

	// Extract the pitch angle from the view matrix.
	m_accumPitchDegrees = Math::radiansToDegrees(asinf(m_viewMatrix[1][2]));

	m_orientation.fromMatrix(m_viewMatrix);
}

void Camera::move(float dx, float dy, float dz) {
	Vector3 eye = m_eye;
	Vector3 forwards;

	forwards = Vector3::cross(WORLD_YAXIS, m_xAxis);
	forwards.normalize();

	eye += m_xAxis * dx;
	eye += WORLD_YAXIS * dy;
	eye += forwards * dz;

	setPosition(eye);
}

void Camera::move(const Vector3 &direction, const Vector3 &amount) {
	m_eye.x += direction.x * amount.x;
	m_eye.y += direction.y * amount.y;
	m_eye.z += direction.z * amount.z;

	updateViewMatrix();
}

void Camera::perspective(float fovx, float aspect, float znear, float zfar) {
	float e = 1.0f / tanf(Math::degreesToRadians(fovx) / 2.0f);
	float aspectInv = 1.0f / aspect;
	float fovy = 2.0f * atanf(aspectInv / e);
	float xScale = 1.0f / tanf(0.5f * fovy);
	float yScale = xScale / aspectInv;

	m_projMatrix[0][0] = xScale;
	m_projMatrix[0][1] = 0.0f;
	m_projMatrix[0][2] = 0.0f;
	m_projMatrix[0][3] = 0.0f;

	m_projMatrix[1][0] = 0.0f;
	m_projMatrix[1][1] = yScale;
	m_projMatrix[1][2] = 0.0f;
	m_projMatrix[1][3] = 0.0f;

	m_projMatrix[2][0] = 0.0f;
	m_projMatrix[2][1] = 0.0f;
	m_projMatrix[2][2] = (zfar + znear) / (znear - zfar);
	m_projMatrix[2][3] = -1.0f;

	m_projMatrix[3][0] = 0.0f;
	m_projMatrix[3][1] = 0.0f;
	m_projMatrix[3][2] = (2.0f * zfar * znear) / (znear - zfar);
	m_projMatrix[3][3] = 0.0f;

	m_viewProjMatrix = m_viewMatrix * m_projMatrix;

	m_fovx = fovx;
	m_aspectRatio = aspect;
	m_znear = znear;
	m_zfar = zfar;
}

void Camera::rotate(float headingDegrees, float pitchDegrees, float rollDegrees) {
	pitchDegrees = -pitchDegrees;
	headingDegrees = -headingDegrees;
	rollDegrees = -rollDegrees;

	rotateFirstPerson(headingDegrees, pitchDegrees);

	updateViewMatrix();
}

void Camera::rotateSmoothly(float headingDegrees, float pitchDegrees, float rollDegrees) {
	headingDegrees *= m_rotationSpeed;
	pitchDegrees *= m_rotationSpeed;
	rollDegrees *= m_rotationSpeed;

	rotate(headingDegrees, pitchDegrees, rollDegrees);
}

void Camera::undoRoll() {
	lookAt(m_eye, m_eye + m_viewDir, WORLD_YAXIS);
}

void Camera::updatePosition(const Vector3 &direction, float elapsedTimeSec) {
	if (m_currentVelocity.magnitudeSq() != 0.0f) {
		Vector3 displacement = (m_currentVelocity * elapsedTimeSec)
				+ (0.5f * m_acceleration * elapsedTimeSec * elapsedTimeSec);

		if (direction.x == 0.0f && Math::closeEnough(m_currentVelocity.x, 0.0f))
			displacement.x = 0.0f;

		if (direction.y == 0.0f && Math::closeEnough(m_currentVelocity.y, 0.0f))
			displacement.y = 0.0f;

		if (direction.z == 0.0f && Math::closeEnough(m_currentVelocity.z, 0.0f))
			displacement.z = 0.0f;

		move(displacement.x, displacement.y, displacement.z);
	}

	updateVelocity(direction, elapsedTimeSec);
}

void Camera::zoom(float zoom, float minZoom, float maxZoom) {
	zoom = std::min(std::max(zoom, minZoom), maxZoom);
	perspective(zoom, m_aspectRatio, m_znear, m_zfar);
}

void Camera::setAcceleration(const Vector3 &acceleration) {
	m_acceleration = acceleration;
}

void Camera::setCurrentVelocity(const Vector3 &currentVelocity) {
	m_currentVelocity = currentVelocity;
}

void Camera::setCurrentVelocity(float x, float y, float z) {
	m_currentVelocity.set(x, y, z);
}

void Camera::setOrientation(const Quaternion &newOrientation) {
	Matrix4 m = newOrientation.toMatrix4();

	m_accumPitchDegrees = Math::radiansToDegrees(asinf(m[1][2]));

	m_orientation = newOrientation;

	lookAt(m_eye, m_eye + m_viewDir, WORLD_YAXIS);

	updateViewMatrix();
}

void Camera::setPosition(const Vector3 &newEye) {
	m_eye = newEye;
	updateViewMatrix();
}

void Camera::setRotationSpeed(float rotationSpeed) {
	m_rotationSpeed = rotationSpeed;
}

void Camera::setVelocity(const Vector3 &velocity) {
	m_velocity = velocity;
}

void Camera::setVelocity(float x, float y, float z) {
	m_velocity.set(x, y, z);
}

void Camera::rotateFirstPerson(float headingDegrees, float pitchDegrees) {
	m_accumPitchDegrees += pitchDegrees;

	if (m_accumPitchDegrees > 90.0f) {
		pitchDegrees = 90.0f - (m_accumPitchDegrees - pitchDegrees);
		m_accumPitchDegrees = 90.0f;
	}

	if (m_accumPitchDegrees < -90.0f) {
		pitchDegrees = -90.0f - (m_accumPitchDegrees - pitchDegrees);
		m_accumPitchDegrees = -90.0f;
	}

	Quaternion rot;

	if (headingDegrees != 0.0f) {
		rot.fromAxisAngle(WORLD_YAXIS, headingDegrees);
		m_orientation = rot * m_orientation;
	}

	if (pitchDegrees != 0.0f) {
		rot.fromAxisAngle(WORLD_XAXIS, pitchDegrees);
		m_orientation = m_orientation * rot;
	}
}

void Camera::updateVelocity(const Vector3 &direction, float elapsedTimeSec) {
	if (direction.x != 0.0f) {
		m_currentVelocity.x += direction.x * m_acceleration.x * elapsedTimeSec;

		if (m_currentVelocity.x > m_velocity.x)
			m_currentVelocity.x = m_velocity.x;
		else if (m_currentVelocity.x < -m_velocity.x)
			m_currentVelocity.x = -m_velocity.x;
	} else {
		if (m_currentVelocity.x > 0.0f) {
			if ((m_currentVelocity.x -= m_acceleration.x * elapsedTimeSec) < 0.0f)
				m_currentVelocity.x = 0.0f;
		} else {
			if ((m_currentVelocity.x += m_acceleration.x * elapsedTimeSec) > 0.0f)
				m_currentVelocity.x = 0.0f;
		}
	}

	if (direction.y != 0.0f) {
		m_currentVelocity.y += direction.y * m_acceleration.y * elapsedTimeSec;

		if (m_currentVelocity.y > m_velocity.y)
			m_currentVelocity.y = m_velocity.y;
		else if (m_currentVelocity.y < -m_velocity.y)
			m_currentVelocity.y = -m_velocity.y;
	} else {
		if (m_currentVelocity.y > 0.0f) {
			if ((m_currentVelocity.y -= m_acceleration.y * elapsedTimeSec) < 0.0f)
				m_currentVelocity.y = 0.0f;
		} else {
			if ((m_currentVelocity.y += m_acceleration.y * elapsedTimeSec) > 0.0f)
				m_currentVelocity.y = 0.0f;
		}
	}

	if (direction.z != 0.0f) {
		m_currentVelocity.z += direction.z * m_acceleration.z * elapsedTimeSec;

		if (m_currentVelocity.z > m_velocity.z)
			m_currentVelocity.z = m_velocity.z;
		else if (m_currentVelocity.z < -m_velocity.z)
			m_currentVelocity.z = -m_velocity.z;
	} else {
		if (m_currentVelocity.z > 0.0f) {
			if ((m_currentVelocity.z -= m_acceleration.z * elapsedTimeSec) < 0.0f)
				m_currentVelocity.z = 0.0f;
		} else {
			if ((m_currentVelocity.z += m_acceleration.z * elapsedTimeSec) > 0.0f)
				m_currentVelocity.z = 0.0f;
		}
	}
}

void Camera::updateViewMatrix() {
	// Reconstruct the view matrix.

	m_viewMatrix = m_orientation.toMatrix4();

	m_xAxis.set(m_viewMatrix[0][0], m_viewMatrix[1][0], m_viewMatrix[2][0]);
	m_yAxis.set(m_viewMatrix[0][1], m_viewMatrix[1][1], m_viewMatrix[2][1]);
	m_zAxis.set(m_viewMatrix[0][2], m_viewMatrix[1][2], m_viewMatrix[2][2]);
	m_viewDir = -m_zAxis;

	m_viewMatrix[3][0] = -Vector3::dot(m_xAxis, m_eye);
	m_viewMatrix[3][1] = -Vector3::dot(m_yAxis, m_eye);
	m_viewMatrix[3][2] = -Vector3::dot(m_zAxis, m_eye);
}
